#include "orbits.hpp"
#include "basePosition.hpp"

stvec stateFromOrbit(orvec orb, int64_t time)
{
	stvec outstate;

	outstate.t   = time;
	outstate.par = orb.par;
	double mu = orb.par.lock()->getMu();

	if( orb.shape() == move_t::ORBIT_E )
	{
		double M;   // mean anomaly
		double E;   // eccentric anomaly
		double E1;
		double nu;  // true anomaly
		double p;
		double px;
		double py;
		double v;
		double vx;
		double vy;
		int64_t t;  // time normalised to fit period

		t = orb.dir * (time - orb.t0);
		M = orb.n()*t;
		E = M;
		do
		{
			E1 = E;
			E  = M + orb.ecc * sin(E1);
		} while( fabs(E - E1) > phys::ERR );

		nu = 2*atan2( sqrt(1+orb.ecc)*sin(E/2) , sqrt(1-orb.ecc)*cos(E/2) );

		p  = orb.sma * (1 - orb.ecc*orb.ecc ) / ( 1 + orb.ecc * cos(nu) );
		px = p*cos(nu);
		py = p*sin(nu);

		// direction of velocity vector
		vx = -py/sqrt(1-orb.ecc*orb.ecc);
		vy = sqrt(1-orb.ecc*orb.ecc)*(px-orb.sma*orb.ecc);
		v  = sqrt(vx*vx+vy*vy);
		vx = vx/v;
		vy = vy/v;


		// magnitude of velocity vector
		v   = orb.dir*sqrt(mu * ( (2/p)-(1/orb.sma) ) );
		vx *= v;
		vy *= v;

		outstate.px =  px*cos(orb.omega) - py*sin(orb.omega);
		outstate.py =  px*sin(orb.omega) + py*cos(orb.omega);
		outstate.vx =  vx*cos(orb.omega) - vy*sin(orb.omega);
		outstate.vy =  vx*sin(orb.omega) + vy*cos(orb.omega);
	}
	else if( orb.shape() == move_t::ORBIT_H )
	{
		double M;   // mean anomaly
		double H;   // hyperbolic eccentric anomaly
		double H1;
		double nu;  // true anomaly
		double p;
		double px;
		double py;
		double v;
		double vx;
		double vy;
		int64_t t;  // time since periaps

		t = orb.dir * (time - orb.t0);
		M = orb.n()*t;

		H = asinh(M/orb.ecc);
		do
		{
			H1 = H;
			H = H1 - ( orb.ecc*sinh(H1) - H1 - M )/( orb.ecc*cosh(H) - 1 );
		} while( fabs(H - H1) > phys::ERR );

		nu = 2*atan2( sqrt(orb.ecc+1)*sinh(H/2), sqrt(orb.ecc-1)*cosh(H/2) );

		p  = orb.sma * (1 - orb.ecc*orb.ecc ) / ( 1 + orb.ecc * cos(nu) );
		px = p*cos(nu);
		py = p*sin(nu);

		// direction of velocity vector
		vx = py/sqrt(orb.ecc*orb.ecc-1);
		vy = sqrt(orb.ecc*orb.ecc-1)*(px-orb.sma*orb.ecc);
		v  = sqrt(vx*vx+vy*vy);
		vx = vx/v;
		vy = vy/v;

		// magnitude of velocity vector
		v   = sqrt(mu * ( (2/p)-(1/orb.sma) ) );
		vx *= v;
		vy *= v;

		outstate.px =  px*cos(orb.omega) + py*sin(orb.omega);
		outstate.py = -px*sin(orb.omega) + py*cos(orb.omega);
		outstate.vx =  vx*cos(orb.omega) + vy*sin(orb.omega);
		outstate.vy = -vx*sin(orb.omega) + vy*cos(orb.omega);
	}
	else if(orb.shape() == move_t::FIXED)
	{
		throw bad_orbit(orb);
	}
	return(outstate);
}

orvec orbitFromState(stvec st)
{
	orvec outorb;

	if(!st.par.expired())
	{
		outorb.par = st.par;
		auto par = st.par.lock();


		double arg = atan2(st.py, st.px);

		double r = st.pabs();
		double v = st.vabs();
		double h = st.px*st.vy - st.py*st.vx;
		double mu= par->getMu();
		int d = sgn(h);

		// eccentricity vector & eccentricity
		double ex  = ( st.vy*h)/mu - st.px/r;
		double ey  = (-st.vx*h)/mu - st.py/r;
		double ecc = sqrt(ex*ex+ey*ey);

		// true anomaly
	//	double nu = acos((ex*st.px+ey*st.py)/(ecc*r));
	//	double nu = atan2(st.py, st.px);
	//	if((st.px*st.vx+st.py*st.vy) < 0)
	//		nu = 2*phys::PI-nu;

		// semi-major axis
		double sma = 1/((2/r)-(v*v/mu));

		// argument of periaps
		double omega = atan2(ey,ex);
		int64_t T;

		double nu = atan2(st.py, st.px) - omega;

		double n = sqrt( fabs(par->getMu()/(sma*sma*sma)));

		if(ecc < 1)
		{
			// elliptical orbit
			std::cout << "elliptical orbit\n";

			// eccentric anomaly
//			double EA = 2*atan(sqrt((1-ecc)/(1+ecc)) * tan(nu/2));
			double EA = 2*atan2(sqrt(1-ecc) * sin(nu/2), (1+ecc) * cos(nu/2));

			// time of periapsis passage
			T = st.t - d*(EA-ecc*sin(EA))/n;


		}
		else
		{
			// hyperbolic orbit
			std::cout << "hyperbolic orbit\n";

			// hyperbolic anomaly
			double HA = 2*atanh(sqrt((ecc-1)/(ecc+1)) * tan(nu/2));

			// time of periapsis passage
			T = st.t - d*(ecc*sinh(HA)-HA)/n;
		}

		outorb.ecc   = ecc;
		outorb.sma   = sma;
		outorb.omega = omega;
		outorb.dir   = d;
		outorb.t0    = T;

		std::cout << toString(outorb);
	}
	else
	{
		throw bad_state(st);
	}

	return(outorb);
}

void pathFromOrbit( conicPath * inpath, orvec orb )
{
	(*inpath).clear();

	if( orb.shape() == move_t::ORBIT_E )
	{
		// calculate full elliptical orbit
		double M;
		double E;
		double E1;
		double nu;
		double p;
		double px;
		double py;
		double periaps;
		double T;
		int64_t t;
		int i;
		int N = screen::ORBITPCOUNT;
		sf::Vector2f tmpVec;
		sf::Vertex   tmpVert;
		std::vector<sf::Vertex> vertexList;
		auto par = orb.par.lock();

		(*inpath).clear();

		// periapsis calculation
		periaps = orb.sma * (1 - orb.ecc*orb.ecc ) / ( 1 + orb.ecc );

		// time where E = pi/2
		T = (phys::PI/2-orb.ecc)/orb.n();

		for( i = 0; i < N/2; i++ )
		{
			t = (int64_t) i*2*(orb.period()-2*T)/N + T;
			M = orb.n()*t;
			E = M;
			do
			{
				E1 = E;
				E = M + orb.ecc * sin(E1);
			} while( fabs(E - E1) > phys::ERR );

			nu = 2*atan2( sqrt(1+orb.ecc)*sin(E/2) , sqrt(1-orb.ecc)*cos(E/2) );

			p  = orb.sma * (1 - orb.ecc*orb.ecc ) / ( 1 + orb.ecc * cos(nu) );
			px = p*cos(nu);
			py = p*sin(nu);

			tmpVec.x = px; // automatically centers on parent
			tmpVec.y = py;
			tmpVert.position = tmpVec;
			vertexList.push_back(tmpVert);
		}

		for( i = 0; i < N/2; i++)
		{
			(*inpath).append(vertexList[i]);
			tmpVec.x = -2*(orb.sma-periaps)-vertexList[i].position.x;
			tmpVec.y = -vertexList[i].position.y;
			tmpVert.position = tmpVec;
			vertexList[i] = tmpVert;
		}

		for( i = 0; i < N/2; i++ )
		{
			(*inpath).append(vertexList[i]);
		}

		(*inpath).append((*inpath)[0]); // completes the circle, ideally

		(*inpath).setRotation(radToDeg(orb.omega));
		(*inpath).setPosition(par->getPosX(), par->getPosY());

	}
	else if (orb.shape() == move_t::ORBIT_H )
	{
		auto par = orb.par.lock();
		double x;
		double y;
		double ymax = par->getSOI()/(-orb.ecc*orb.sma);
		double ymin = 1e5/sqrt(( -par->getMu()/orb.sma )*( 1+orb.ecc )/( orb.ecc-1 ));
		double k; // scaling factor for y
		double up;     // bounds for binary search
		double down;
		double trial;
		int N = screen::ORBITPCOUNT/2;
		int i;
		sf::Vector2f tmpVec;
		sf::Vertex   tmpVert;
		std::vector<sf::Vertex> vertexList;


		// finds "appropriate" scaling constant
		up   = 100;
		down = 0.99;
		do
		{
			k = 0.5*(up+down);
			trial = pow(k,N+1) - 1 - (ymax/ymin)*(k-1);
			if(trial > 0)
			{
				up = k;
			}
			else
			{
				down = k;
			}
		} while( fabs(up - down) > phys::ERR );

		for( i = 0; i < N; i++)
		{
			y = pow(k,i)*ymin;
			x = -sqrt( (y*y)/(orb.ecc*orb.ecc-1) + orb.sma*orb.sma ) - orb.sma*orb.ecc;
			tmpVec.x = x;
			tmpVec.y = y;
			tmpVert.position = tmpVec;
			vertexList.push_back(tmpVert);
		}

		for( i = 1; i <= N; i++)
		{
			(*inpath).append(vertexList[N-i]);
			tmpVec.x =  vertexList[N-i].position.x;
			tmpVec.y = -vertexList[N-i].position.y;
			tmpVert.position = tmpVec;
			vertexList[N-i] = tmpVert;
		}

		for( i = 0; i < N; i++ )
		{
			(*inpath).append(vertexList[i]);
		}

		(*inpath).setRotation(radToDeg(orb.omega));
		(*inpath).setPosition(par->getPosX(), par->getPosY());
	}

	// empty path is generated if orbit is fixed
}



















stvec::stvec()
{
	px = 0;
	py = 0;
	vx = 0;
	vy = 0;
	par.reset();
	t = phys::globTime;
}

stvec::stvec(double inpx, double inpy)
{
	px = inpx;
	py = inpy;
	vx = 0;
	vy = 0;
	par.reset();
}

stvec::stvec(double inpx, double inpy, double invx, double invy)
{
	px = inpx;
	py = inpy;
	vx = invx;
	vy = invy;
	par.reset();
}

double stvec::pabs()
{
	return(sqrt(px*px+py*py));
}

double stvec::vabs()
{
	return(sqrt(vx*vx+vy*vy));
}


orvec::orvec()
{
	par.reset();
}

int64_t orvec::period()
{
	if( shape() == move_t::ORBIT_E)
	{
		auto parent = par.lock();
		return(2*phys::PI*sqrt( (sma*sma*sma)/parent->getMu() ));
	}
	else
	{
		return(-1);
	}
}

double orvec::n()
{
	if( shape() != move_t::FIXED )
	{
		auto parent = par.lock();
		return(sqrt( fabs(parent->getMu()/(sma*sma*sma)) ));
	}

}

unsigned char orvec::shape()
{
	if( sma > 0 && ecc >= 0 && ecc < 1 && dir != 0 && !par.expired() )
	{
		return(move_t::ORBIT_E);
	}
	else if( sma < 0 && ecc > 1 && dir != 0 && !par.expired())
	{
		return(move_t::ORBIT_H);
	}
	else
	{
		return(move_t::FIXED);
	}
}




std::string toString(stvec invec)
{
	std::stringstream stream;
	stream << "p  : " << invec.px << " " << invec.py << " " << invec.pabs() << "\n";
	stream << "v  : " << invec.vx << " " << invec.vy << " " << invec.vabs() << "\n";
	stream << "t  : " << invec.t << "\n";
	stream << "par: ";
	if(!invec.par.expired())
		stream << "VALID";
	else
		stream << "INVALID";
	stream << "\n";

	return(stream.str());
}

std::string toString(orvec invec)
{
	std::stringstream stream;
	stream << "sma  : " << invec.sma << "\n";
	stream << "ecc  : " << invec.ecc << "\n";
	stream << "omega: " << invec.omega << "\n";
	stream << "t0   : " << invec.t0 << "\n";
	stream << "par  : ";
	if(!invec.par.expired())
		stream << "VALID";
	else
		stream << "INVALID";
	stream << "\n";

	return(stream.str());
}

stvec operator+(stvec a, const stvec& b)
{
	a.px += b.px;
	a.py += b.py;
	a.vx += b.vx;
	a.vy += b.vy;

	return(a);
}

stvec operator-(stvec a, const stvec& b)
{
	a.px -= b.px;
	a.py -= b.py;
	a.vx -= b.vx;
	a.vy -= b.vy;

	return(a);
}



bad_orbit::bad_orbit(orvec inorbit)
{
	orbit = inorbit;
}

const char* bad_orbit::what() const noexcept
{
	std::stringstream stream;
	stream << "bad orbit detected\n" << toString(orbit);
	return( stream.str().c_str() );
}

bad_state::bad_state(stvec instate)
{
	state = instate;

}

const char* bad_state::what() const noexcept
{
	std::stringstream stream;
	stream << "bad state detected\n" << toString(state);
	return( stream.str().c_str() );
}


